/*
 * @Author: xiaoxiaotaoya 1712331226@qq.com
 * @Date: 2024-12-22 14:17:28
 * @LastEditors: xiaoxiaotaoya 1712331226@qq.com
 * @LastEditTime: 2024-12-22 21:48:46
 * @FilePath: /trobot_ws/src/controller_modules/mc_path_planner/src/complete_coverage_path/ccpp_planner.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <mc_path_planner/complete_coverage_path/ccpp_planner.h>


namespace ccpp{

CompleteCoverPlanner::CompleteCoverPlanner(boost::shared_ptr<MapBase> map_handler_ptr):mapHandlerPtr_(map_handler_ptr){
    ploygonPub_ = nh_.advertise<geometry_msgs::PolygonStamped>("/coverage_polygon", 1, true);
    startPointPub_ = nh_.advertise<geometry_msgs::PointStamped>("/coverage_start_point", 1, true);
    pathPub_ = nh_.advertise<nav_msgs::Path>("/coverage_path", 1, true);
    edgePathPub_ = nh_.advertise<nav_msgs::Path>("/coverage_edge_path", 1, true);
    smoothPathPtr_ = boost::make_shared<FivePointGradientDescent>(mapHandlerPtr_);
    edgePathPlanner_ = boost::make_shared<ccpp::EdgePathPlanner>(mapHandlerPtr_, smoothPathPtr_);
    sprialPathPlanner_ = boost::make_shared<ccpp::SprialPathPlanner>(mapHandlerPtr_, smoothPathPtr_);
}

CompleteCoverPlanner::~CompleteCoverPlanner(){

}



// 全覆盖路径生成 - 基于多边形轮廓
bool CompleteCoverPlanner::genPathWithPolygon(geometry_msgs::Point start, vector<geometry_msgs::Point> polygon, PathType path_type, CoveragePath& path){
    // 发布轮廓信息
    geometry_msgs::PolygonStamped polygon_stamp;
    polygon_stamp.header.stamp = ros::Time::now();
    polygon_stamp.header.frame_id = "map";
    for(auto point : polygon){
        geometry_msgs::Point32 point_32;
        point_32.x = point.x;
        point_32.y = point.y;
        polygon_stamp.polygon.points.emplace_back(point_32);
    }
    ploygonPub_.publish(polygon_stamp);

    // 发布起点数据
    geometry_msgs::PointStamped point_stamp;
    point_stamp.header.stamp = ros::Time::now();
    point_stamp.header.frame_id = "map";
    point_stamp.point = start;
    startPointPub_.publish(point_stamp);

    if(!mapHandlerPtr_){
        ROS_ERROR("please set map data!");
        return false;
    }
    LOG(INFO) << "[genPathWithPolygon] path_type: " << edgePathPlanner_->toString(path_type);

    CoveragePath coverage_path;
    switch(path_type){
        case PathType::EDGE_PATH:
            {
                edgePathPlanner_->genEdgePathWithPolygon(start, polygon, coverage_path);
            }
            break;
        case PathType::SPRIAL_AND_BOW_SHAPED_PATH:
            {
                sprialPathPlanner_->genSprialPathWithPolygon(start, polygon, coverage_path);
            }
            break;
        case PathType::SPRIAL_AND_ARC_SHAPED_PATH:
            {
                edgePathPlanner_->genEdgePathWithPolygon(start, polygon, coverage_path);
                sprialPathPlanner_->genSprialPathWithPolygon(start, polygon, coverage_path);
            }
            break;
        default:
            break;
    }


    vector<Point2f> coverage_points_2f;
    for(auto points : coverage_path.coverage_path){
        coverage_points_2f.insert(coverage_points_2f.end(), 
            points.begin(), points.end());
    }
    nav_msgs::Path path_msg = msg_convert::toPath(coverage_points_2f, "map");
    pathPub_.publish(path_msg);

    vector<Point2f> edge_points_2f;
    for(auto points : coverage_path.edge_path){
        edge_points_2f.insert(edge_points_2f.end(), 
            points.begin(), points.end());
    }
    nav_msgs::Path edge_path_msg = msg_convert::toPath(edge_points_2f, "map");
    edgePathPub_.publish(edge_path_msg);
    // // 路径点转换
    // for(auto pose_stamp : path_msg.poses){
    //     edge_path.emplace_back(pose_stamp.pose);
    // }
    path = coverage_path;
    return true;
}
        
};  //namespace